#ifndef TOALOCALIZATION_H
#define TOALOCALIZATION_H

#include <QObject>
#include <QVector>
#include <QPointF>
#include <QtMath>
#include <QDebug>
#include <QMainWindow>
#include<QWebSocketServer>
#include<QWebSocket>
#include"websocket.h"
#include "fireman.h"
#include "Utils.h"


class TOALocalization : public QObject
{
    Q_OBJECT

public:
    explicit TOALocalization(QObject *parent = nullptr);
    WebSocket ws;
    FireMan fm=FireMan(1);
    void startCalculation();
    bool calculateNextPoint(QString id);
    void resetCalculation();
    void updateMeasurements(const QVector<double> &distances);
    // 获取结果数据
    const QVector<QVector<double>>& getIdealTrajectory() const { return u0; }
    const QVector<QVector<double>>& getCalculatedPositions() const { return U; }
    const QVector<QVector<double>>& getIterationErrors() const { return ei; }
    const QVector<QPointF>& getBaseStations() const { return Si; }
    int getCurrentPointIndex() const { return currentPoint; }
signals:
    void pointCalculated(QString index, double x, double y);
private:
    void calculatePoint(int i,QString id);

    int n;  // 最大迭代次数
    int m;  // 坐标点数量
    int M;  // 基站数量
    int currentPoint = 0;

    QVector<QPointF> Si; // 基站位置
    QVector<QVector<double>> ni; // 测量误差矩阵
    QVector<QVector<double>> ri; // TOA测量值矩阵
    QVector<QVector<double>> C;  // 迭代参数矩阵
    QVector<QVector<double>> U;  // 定位结果矩阵
    QVector<QVector<double>> ei; // 迭代误差矩阵
    QVector<QVector<double>> ek; // 迭代误差矩阵
    QVector<QVector<double>> u0; // 理想轨迹
};

#endif // TOALOCALIZATION_H
